/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __JOINT_MATH_H
#define __JOINT_MATH_H

#define link1_len				(32.80f)
#define link2_len				(89.0f)
#define link3_len				(138.0f)

#define PI																							(3.141592654f)
#define limit(in, max, min)															(in > max ? max : in < min ? min : in)
#define pow2(x) powf(x,2)
#define Cosine_BegAngle(_ortho1, _ortho2, _opposite)		(acos((pow(_ortho1, 2)+pow(_ortho2, 2)-pow(_opposite, 2)) \
																												/ (2.0f * _ortho1 * _ortho2)))
																												
union _point_t{
	float axis[3];
	struct {
		float x;
		float y;
		float z;
	};
};

#include "bits/stdint-uintn.h"
#include "math.h"

typedef union _point_t point_t;

typedef struct {
	float angle[3];
	
	float joint1_angle_err;
} ik_t;

void ik(ik_t *ik, union _point_t point);

#endif /* __JOINT_MATH_H */